from MiniBotControlLib import MiniBotControlLib
from MiniBotAlgorithmLib import MiniBotAlgorithmLib
import time
from ctypes import *

######################################################################
######################################################################
####################### 四轮移动小车（麦轮) ###########################
######################################################################
######################################################################

#声明动态库辅助对象
mclib = MiniBotControlLib()
malib = MiniBotAlgorithmLib()

#正在运行机器人ID
m_RunningRobotId=-1

print("Welcome to ManipulatorRobot Sample....")
print("Enter any key to scan robots ....")
input("")



#数字输入辅助方法
def inputNum(type=int,content=''):
  data = input(content)
  try:
    inputData = type(data)
    return inputData
  except:
   #pass
   return -1


######################################################################
################# 算法Api模块关节设置回调函数声明 ####################
######################################################################

# MiniBotAlgorithmLib算法中设置关节角度的回调函数定义
# 参数：模块ID，角度（I:正负180；T：正负90；G：0到90）
def setPosition(id,pos):
 mclib.SetPosition(id,pos)


def setPositionWithSpeed(id,pos,speed):
 mclib.SetPositionWithSpeed(id,pos,speed)

 
# MiniBotAlgorithmLib算法中以指定速度百分比设置关节角度的回调函数定义
# 参数：模块ID，角度（I:正负180；T：正负90；G：0到90），速度百分比
def setMultiPosition(id,pos,count):
 ids=[]
 angles= []
 c = 0
 while c < count:
     ids.append(id[c])
     angles.append(pos[c])
     c=c+1
 mclib.SetPositionSync(ids,angles)


# MiniBotAlgorithmLib算法中设置多个关节角度的回调函数定义
# 参数：模块ID数组，角度数组，数组长度
def setMultiPositionWithSpeed(ids,pos,count,speed):
 for id in ids:
     setPositionSpeed(id,speed)
 mclib.SetPositionSync(ids,pos)


# MiniBotAlgorithmLib算法中以指定速度设置多个关节角度的回调函数定义
# 参数：模块ID数组，角度数组，数组长度，速度百分比
def setPositionSpeed(id,speed):
 mclib.SetPositionSpeed(id,speed)


#MiniBotAlgorithmLib算法中设置关节速度的回调函数定义
#参数：模块ID，速度百分比（0到100）
def setRunningSpeed(id,speed):
 mclib.SetRunningSpeed(id,speed)

#MiniBotAlgorithmLib算法中设置关节是否使能的回调函数定义
#参数：模块ID，是否使能（转矩是否生效）
def setTorqueEnable(id,enable):
 mclib.EnableTorque(id,enable)

#MiniBotAlgorithmLib算法中设置模块运行模式的回调函数定义
#参数：模块ID，模式值(0:位置模式；1:速度模式；2:PWM开环调速度模式；)
def setMode(id,mode):
 mclib.SetMotorMode(id,mode)


######################################################################
################# 全局变量定义 ####################
######################################################################
#扫描在线机器人Start
robotsIps = []
robotsCount = 0

#当前连接的机器人IP与端口Port
robotIp = ""
robotPort = 0

#当前机器人的ID序列
_moduleIds = []

#算法机器ID
_robotId = -1


######################################################################
####################### 机器人连接及模块扫描 ###########################
######################################################################
while(1):
    cmd = 0
    if(robotsCount > 0):
        print("Enter the robot index (ex: 1)...[Input 0 to scan again]：")
        cmd = inputNum()
        
    if(cmd == 0):
         print("Scaning the robots......")

         #扫描在线机器人
         robotsIps = mclib.ScanRobots()
         robotsCount = len(robotsIps)
         print("Found robots：" + str(robotsCount))
         i = 0
         while i < robotsCount:
           print(str(i + 1) + ":" + str(robotsIps[i]))
           i = i + 1

    elif(cmd > 0 and cmd <= robotsCount):
        robotAddr = robotsIps[cmd - 1]
        ipPort = robotAddr[:robotAddr.index(",")]     #IP与端口
        ip = ipPort
        port = 8899
        if(ipPort.find(":") > 0):
           ip = ipPort[:ipPort.index(":")]
           port = int(ipPort[ipPort.index(":") + 1:])

        #连接机器人设备
        res = mclib.Connect(ip,port)
        #res = mclib.ConnectWithTimeOut(ip,port,200)
        if(res > 0):
            print("Connected")
            robotIp = ip
            robotPort = port
            break
        else:
            print("Connect Fail")


######################################################################
################# 机器人模块ID扫描 ##################################
######################################################################
while(1):
    ids = mclib.ScanId(20,50)
    idsCount = len(ids[0])
    print("Modules Total Count： " + str(idsCount))
    print("Followed：")

    c = 0
    while c < idsCount:
         print("Id： " + str(str(ids[0][c])) + "  Position:" + str(ids[1][c]))
         c = c + 1

    #默认提示扫描机器人模块ID列表
    cmd = 0
    print("Any Key to continue....... (Input 0 to scan again)")
    cmd = inputNum()
    if(cmd != 0):
        break


######################################################################
################# 机器人模块ID配置 ##################################
######################################################################
#输入四轮移动小车（麦轮)ID次序
print("The id sequence above may be incorrect")
print("Please enter the Mcrum Wheel Car Robot ids in sequence（L1,L4,R1,R4）.")
_moduleIds = []

print("Please enter the Mcrum Wheel id at L1.")
_moduleIds.append(inputNum())
print("Please enter the Mcrum Wheel id at L4.")
_moduleIds.append(inputNum())
print("Please enter the Mcrum Wheel id at R1.")
_moduleIds.append(inputNum())
print("Please enter the Mcrum Wheel id at R4.")
_moduleIds.append(inputNum())

mIds= [0]*10
mIds[2] = _moduleIds[0];
mIds[5] = _moduleIds[1];
mIds[6] = _moduleIds[2];
mIds[9] = _moduleIds[3];

print("Ids  completed")
print("Bind to creat a car robot...")

######################################################################
################# 机器人算法绑定 ##################################
######################################################################

#回调函数类型声明
setPositionCallback_type = CFUNCTYPE(None,c_ubyte, c_double)
setPositionWithSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_double,c_int)
setMultiPositionCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int)
setMultiPositionWithSpeedCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int , c_int)
setSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_int)
setTorqueEnableCallback_type = CFUNCTYPE(None,c_ubyte, c_bool)
setModeCallback_type = CFUNCTYPE(None,c_ubyte, c_int)

#回调函数
setPositionCallback = setPositionCallback_type(setPosition)
setPositionWithSpeedCallback = setPositionWithSpeedCallback_type(setPositionWithSpeed)
setMultiPositionCallback = setMultiPositionCallback_type(setMultiPosition)
setMultiPositionWithSpeedCallback = setMultiPositionWithSpeedCallback_type(setMultiPositionWithSpeed)
setPositionSpeedCallback = setSpeedCallback_type(setPositionSpeed)
setRunningSpeedCallback = setSpeedCallback_type(setRunningSpeed)
setTorqueEnableCallback = setTorqueEnableCallback_type(setTorqueEnable)
setModeCallback = setModeCallback_type(setMode)

#5表示小车模式为四个并向麦克朗姆轮
_robotId=malib.AddRobot(1,mIds,5)
#绑定关节处理函数
malib.BindRobotModuleApis(_robotId,setPositionCallback,setPositionWithSpeedCallback,setMultiPositionCallback,setMultiPositionWithSpeedCallback,setPositionSpeedCallback,setRunningSpeedCallback,setTorqueEnableCallback,setModeCallback)


######################################################################
################# 机器人操作控制开始 ##################################
######################################################################


while(1):
    print("1.Go Forward")
    print("2.Go Back")
    print("3.Stop")
    print("4.Turn Left")
    print("5.Turn Right")
    print("6.Go Left")
    print("7.Go Right")
    print("8.Rotate Angles")
    cmd = inputNum()

    if(cmd == 8):
        print("8.Rotate Angles")
        angle=inputNum()
        malib.SendCmd(_robotId,cmd,angle)

    else:
        malib.SendCmd(_robotId, cmd, 0);

print( "Invoked complete")
print("Enter any key to continue ....")
input()
    


